#include "is_homography_R.h"
#include<cmath>

#define IS_DEBUG 0

extern "C" void prints(char *format,...);

IsHomographyR::IsHomographyR()
{
    //kalmanType imgS = 1;
    //kalmanType imgTheta = 0;
    //kalmanType imgT[2] = {0, 0};
    //kalmanType imgSThetaTArray[4] = {imgS, imgTheta, imgT[0], imgT[1]};
	//memset(imgHomographyMat, 0, 9*sizeof(kalmanType));
}

IsHomographyR::~IsHomographyR()
{
}
/*
 * ABOUT : This function computes the Homography Matrix.
 * ARGS : It takes curQuar, prevQuat, curCamera and prevCamera.
 * RETURN : An array repsenting homography matrix.
 */
void IsHomographyR::getHomographyMat(kalmanType *pArrayTwoQuaternion, kalmanType *pArrayCamInternalMatrix)
{
    //kalmanType* pimgHomographyMat = IsHomographyR::imgHomographyMat;
    // creating intermediate variables.
    kalmanType curQ[QUAT_LENGTH];       // current quaternion.
    kalmanType prevQ[QUAT_LENGTH];      // previous quaternion.
    kalmanType curK[K_MATRIX_LENGTH];   // current camera matrix.
    kalmanType prevK[K_MATRIX_LENGTH];  // previous camera matrix.
    
    kalmanType* pQ;
    kalmanType* pK;
    pQ = pArrayTwoQuaternion; 
      
    // Use these arrays to construct curQuaternion and PrevQuaternions.
    for(int i = 0; i < 4; i++)
    {
        prevQ[i] = *pQ;
        pQ++;
    }
    pQ = pArrayTwoQuaternion;
    pQ = pQ + 4;
    for(int i = 0; i < 4; i++)
    {
        curQ[i] = *pQ;
        pQ++;
    }
        // Normalize them.
#if IS_DEBUG
    // DEBUG : printing unnormalised quaternions.
    prints("\n--DEBUG: Printing unnormalised quaternions.\n");
    for(int i = 0; i < 4; i++)
    {
        prints("curQ[%d]: %lf", i, curQ[i]);
    }
    for(int i = 0; i < 4; i++)
    {
		prints("prevQ[%d]: %lf", i, prevQ[i]);
    }
#endif
    IsQuaternionR::normalizeQuatR(curQ);
    IsQuaternionR::normalizeQuatR(prevQ);
    
    // Step 1 : Constust PrevCamera and CurrentCamera Matrix from array
    pK = pArrayCamInternalMatrix; // previous camera
    for(int i = 0; i < 9; i++)
    {
        prevK[i] = *pK;
        pK++;
    }
    pK = pArrayCamInternalMatrix;
    pK = pK + 9;
    for(int i = 0; i < 9; i++)
    {
        curK[i] = *pK;
        pK++;
    }
#if 0
    printf("\n--DEBUG: Printing camera matrix.\n");
    for(int i = 0; i < 9; i++)
    {
        cout<<curK[i]<<"\t";
    }
    printf("\n");
    for(int i = 0; i < 9; i++)
    {
        cout<<prevK[i]<<"\t";
    }

#endif
    /* 
     * Now we calculate the rotation Matrix between prevQ and curQ
    */
    // Two rotation matrix for both of the quats. curQuaternions and prevQuaternions.
    kalmanType rotMat1[R_MATRIX_LENGTH];
    kalmanType rotMat2[R_MATRIX_LENGTH];
    
    // calculate the first rotation matrix.
    IsQuaternionR::quatToRotationMat(curQ);
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        rotMat1[i] = IsQuaternionR::myRotationMatrix[i];
    }
    //Calculate the second rotation matrix.
    IsQuaternionR::quatToRotationMat(prevQ);
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        rotMat2[i] = IsQuaternionR::myRotationMatrix[i];
    }  
#if IS_DEBUG
    prints("-DEBUG: Printing normalised quats.\n\r"); 
    for(int i = 0; i < 4; i++)
    {
        prints("curQ[%d] : %lf ", i, curQ[i]);
    }
    for(int i = 0; i < 4; i++)
    {
        prints("prevQ[%d] : %lf ", i, prevQ[i]);
    }
#endif

#if IS_DEBUG

    prints("\n--DEBUG: Printing rotation Matrix.\n\r");
    for(int i=0; i<9; i++)
    {
        prints("rotMat1[%d] : %lf", i, rotMat1[i]);
    }
    prints("\n\r");
    for(int i=0; i<9; i++)
    {
	   prints("rotMat2[%d] : %lf", i, rotMat2[i]);
    }
#endif

    //STEP 3: Calculate the inverse of rotMat1
    kalmanType invRotMat1[R_MATRIX_LENGTH];
    IsMatrixR::calcInverseMat(rotMat1, R_MATRIX_LENGTH);
    // construct this rotation matrix.
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        invRotMat1[i] = IsMatrixR::imgInverseMatrix[i];
    }
#if IS_DEBUG
    prints("\n\rDEBUG : main() - rotation and its inverse mat.\n");
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        prints("rotMat1[%i]: %lf", i, rotMat1[i]);
    }
    prints("\n\r");
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        prints("invRotMat1[%i]: %lf", i, invRotMat1[i]);
    }
#endif
    // Step 4: Multiply invRotMat1 and rotMat2
    kalmanType rotMat[R_MATRIX_LENGTH];
    //kalmanType* pMultMat = IsMatrixR::imgMultMatrix;
    IsMatrixR::multMatrix(invRotMat1, rotMat2, 3); 
    // construct this rotation matrix.
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        rotMat[i] = IsMatrixR::imgMultMatrix[i];
    }
   
#if IS_DEBUG
    prints("\n\rDEBUG : main() - multiplication of matrices.\n\r");
    for(int i = 0; i < R_MATRIX_LENGTH; i++)
    {
        prints("rotMat[%i], %lf", i, rotMat[i]);
    }
#endif
    
   /* This block will calculate the homography matrix. */
    // Inverse curK.
    kalmanType invPrevK[R_MATRIX_LENGTH];
    IsMatrixR::calcInverseMat(prevK, K_MATRIX_LENGTH);

    // copy this into local matrix.
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        invPrevK[i] = IsMatrixR::imgInverseMatrix[i];
    }
#if IS_DEBUG
    prints("\n\r--DEBUG : Printing camera matrices.\n\r");
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        prints("rotMat[%i], %lf", i, rotMat[i]);
    }
   
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        prints("prevK[%i]: %lf", i ,prevK[i]);
    }
    prints("\n\r--DEBUG : Printing inverse matrix of curK.\n\r");
    for(int i = 0 ; i < R_MATRIX_LENGTH; i++)
    {
        prints("invPrevK[%i]: %lf" ,i , invPrevK[i]);
    }
#endif
    // Final calculation.
	multMatrix(rotMat, invPrevK, 3);
    multMatrix(curK, IsMatrixR::imgMultMatrix, 3);
    // copy it into class member matrix.

	for(int i = 0; i < H_MATRIX_LENGTH; i++)
    {
        imgHomographyMat[i] = imgMultMatrix[i];
    }

#if IS_DEBUG
    prints("\r\n--DEBUG : Printing Homography matrices. Inside getHomography\r\n");
    for(int i = 0 ; i < H_MATRIX_LENGTH; i++)
    {
        prints("IsHomoMat[%i]: %lf", i, imgHomographyMat[i]);
    }
	prints("\r\n");
#endif
 
}

/* 
 * Decompose the homography matrix.
 * ABOUT : Into S, theta and T.
 */
void IsHomographyR::decomposeHomographyMat(kalmanType* pHomographyMat)
{
    // construct a local array to hold these values.
#if IS_DEBUG
	for(int i = 0; i < H_MATRIX_LENGTH; i++)
    {
        prints(" H[%i]: %lf", i, IsHomographyR::imgHomographyMat[i]);
	}
#endif
	pHomographyMat = IsHomographyR::imgHomographyMat;
	kalmanType H[H_MATRIX_LENGTH]; //H_MATRIX_LENGTH
#if IS_DEBUG
	prints("Homography Mat here is : \n\r");
	for(int i = 0; i < H_MATRIX_LENGTH; i++)
    {
        prints(" H[%i]: %lf", i, pHomographyMat[i]);
	}
#endif
    for(int i = 0; i < H_MATRIX_LENGTH; i++)
    {
        H[i] = pHomographyMat[i];
	}
	

    // Local variable to be used in calculations.
    kalmanType h33 = H[8];
    kalmanType v[2] = {H[6], H[7]};
    kalmanType t[2] = {H[2]/h33, H[5]/h33};
    kalmanType tvMat[4] = {t[0]*v[0], t[0]*v[1], t[1]*v[0], t[1]*v[1]}; // OK.

    kalmanType matB[4] = {H[0], H[1], H[3], H[4]};
    for(int i = 0; i < 4; i++)
    {
        matB[i] =  matB[i] - tvMat[i];  // Tested O.K. with Matlab.
    }

    /*
     * We now solve these equations.
     * AX = B
     * where A = [cos x,    0,      0
                  0    ,  cos x, -sin x
                  0    , sin x, cos x]
     * and B = [ 
                matA[0]
                matA[1]
                matA[2]
               ]

     * x = atan(matA[2]/matA[0];
     */
    kalmanType x = atan(matB[2]/matB[0]); // theta = tan^-1(c/a)              
    /*
    cout<<"\nPrinting Theta : "<<x;
    */
    kalmanType A[K_MATRIX_LENGTH] = { 
                                      cos(x), 0, 0,
                                      0, cos(x), -sin(x),
                                      0, sin(x), cos(x)
                                    };
    kalmanType B[3] = {
                        matB[0],
                        matB[1],
                        matB[3]
                      };
    kalmanType invA[K_MATRIX_LENGTH];
    kalmanType X[3] = {0 , 0, 0}; // array to hold variables.
    //kalmanType* pX = X;

    IsMatrixR::calcInverseMat(A, K_MATRIX_LENGTH);
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        invA[i] = IsMatrixR::imgInverseMatrix[i]; // Tested ok with MATLAB.
    }
    // Caculate X = inv(A)*B
    for(int i = 0; i < 3; i++)              // Tested O.K. with Matlab.
    {
        for(int j = 0; j < 3; j++)
        {
            kalmanType temp = invA[j+3*i]*B[j];
            X[i] = X[i] + temp;
        }
    }

    /* Caculate s, theta and T. */
    IsHomographyR::imgS = sqrt(X[0]*X[2]);
    IsHomographyR::imgTheta = x;
    IsHomographyR::imgT[0] =t[0]; IsHomographyR::imgT[1] =t[1];
    
#if 0
    
    cout<<"\nFOR DEBUGGING ..\n\n";
    cout<<"\nDEBUG: Decompose homography...\n";
    cout<<"\n\n";
    for(int i = 0; i < 4; i++)
    {
        cout<<matB[i]<<"\t";
        //cout<<tvMat[i]<<"\t"; // Tested OK with MATLAB
    }
    
    cout<<"\n\n Mat and invMat"<<endl;
    /*
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        cout<<H[i]<<"\t";
    }
    */
    cout<<"\n\n";
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        cout<<A[i]<<"\t";
     }
    cout<<"\n\n";
    for(int i = 0; i < K_MATRIX_LENGTH; i++)
    {
        cout<<invA[i]<<"\t";
    }
    cout<<"\n\n Variables";
    for(int i = 0; i < 3; i++)
    {
        cout<<X[i]<<"\t";
    }
    cout<<"\n\nEnd ...";

#endif
    IsHomographyR::imgSThetaTArray[0] = IsHomographyR::imgS;
    IsHomographyR::imgSThetaTArray[1] = IsHomographyR::imgTheta;
    IsHomographyR::imgSThetaTArray[2] = IsHomographyR::imgT[0];
    IsHomographyR::imgSThetaTArray[3] = IsHomographyR::imgT[1];  
    
}
